/**
 * @file my_matrix3x3.h
 * @brief 矩阵
 * @details 矩阵的构造以及多种形式的转化
 * @author all
 * @date 2023/11/7
 * @version 1.0
 * @par 更新记录：
 * - 2023/11/3 创建文档
 * - 2023/11/7 添加注释
*/
/********************<start>**********************/
#ifndef MY_MATRIX3X_H
#define MY_MATRIX3X_H

/********************<include>********************/
#include"my_vector.h"
#include"my_quaternion.h"

namespace my_tf
{
/********************<class>**********************/
    /**
     * @brief 3x3矩阵类
     */
    class Matrix3x3{
        Vector3 m_el[3];
    public:
        /**
         * @brief 默认构造函数
         */
        Matrix3x3(){}
        /**
         * @brief 使用四元数构造矩阵
         * @param q 四元数
         */
        explicit Matrix3x3(const Quaternion& q) { setRotation(q); }
        /**
         * @brief 获取矩阵的某一行作为向量的可变引用
         * @param i 行索引（从0开始）
         * @return 向量的可变引用
         */
        inline Vector3&  operator[](int i)
        { 
            
            myAssert(0 <= i && i < 3);
            return m_el[i]; 
        }
        /**
         * @brief 获取矩阵的某一行作为向量的常量引用
         * @param i 行索引（从0开始）
         * @return 向量的常量引用
         */
        inline const Vector3& operator[](int i) const
        {
            myAssert(0 <= i && i < 3);
            return m_el[i]; 
        }
        /**
         * @brief 设置矩阵的旋转部分为给定的四元数
         * @param q 四元数
         */
        void setRotation(const Quaternion& q) 
        {
            double d = q.length2();
            myAssert(d != double(0.0));
            double s = double(2.0) / d;
            double xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
            double wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
            double xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
            double yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
            setValue(double(1.0) - (yy + zz), xy - wz, xz + wy,
                xy + wz, double(1.0) - (xx + zz), yz - wx,
                xz - wy, yz + wx, double(1.0) - (xx + yy));
        }
        /**
         * @brief 设置矩阵的值（按行主序）
         * @param xx 左上角元素
         * @param xy 上中间元素
         * @param xz 右上角元素
         * @param yx 中左元素
         * @param yy 中间元素
         * @param yz 中右元素
         * @param zx 左下角元素
         * @param zy 下中间元素
         * @param zz 右下角元素
         */
        void setValue(const double& xx, const double& xy, const double& xz, 
		const double& yx, const double& yy, const double& yz, 
		const double& zx, const double& zy, const double& zz)
        {
            m_el[0].setValue(xx,xy,xz);
            m_el[1].setValue(yx,yy,yz);
            m_el[2].setValue(zx,zy,zz);
        }
        /**
         * @brief 将矩阵表示为YXZ欧拉角形式，与setEulerYPR函数互逆
         * @param yaw 绕Z轴的偏航角
         * @param pitch 绕Y轴的俯仰角
         * @param roll 绕X轴的滚转角
         * @param solution_number 解的编号，取值为1或2
         */
        void getEulerYPR(double& yaw, double& pitch, double& roll, unsigned int solution_number = 1) const
        {
            struct Euler
            {
                double yaw;
                double pitch;
                double roll;
            };
            Euler euler_out;
            Euler euler_out2; //second solution
            //get the pointer to the raw data
            // Check that pitch is not at a singularity
            // Check that pitch is not at a singularity
            if (tfFabs(m_el[2].x()) >= 1)
            {
                euler_out.yaw = 0;
                euler_out2.yaw = 0;
        
                // From difference of angles formula
                if (m_el[2].x() < 0)  //gimbal locked down
                {
                double delta = tfAtan2(m_el[0].y(),m_el[0].z());
                    euler_out.pitch = TFSIMD_PI / double(2.0);
                    euler_out2.pitch = TFSIMD_PI / double(2.0);
                    euler_out.roll = delta;
                    euler_out2.roll = delta;
                }
                else // gimbal locked up
                {
                double delta = tfAtan2(-m_el[0].y(),-m_el[0].z());
                    euler_out.pitch = -TFSIMD_PI / double(2.0);
                    euler_out2.pitch = -TFSIMD_PI / double(2.0);
                    euler_out.roll = delta;
                    euler_out2.roll = delta;
                }
            }
            else
            {
                euler_out.pitch = - tfAsin(m_el[2].x());
                euler_out2.pitch = TFSIMD_PI - euler_out.pitch;

                euler_out.roll = tfAtan2(m_el[2].y()/tfCos(euler_out.pitch), 
                    m_el[2].z()/tfCos(euler_out.pitch));
                euler_out2.roll = tfAtan2(m_el[2].y()/tfCos(euler_out2.pitch), 
                    m_el[2].z()/tfCos(euler_out2.pitch));

                euler_out.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out.pitch), 
                    m_el[0].x()/tfCos(euler_out.pitch));
                euler_out2.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out2.pitch), 
                    m_el[0].x()/tfCos(euler_out2.pitch));
            }

            if (solution_number == 1)
            { 
                yaw = euler_out.yaw; 
                pitch = euler_out.pitch;
                roll = euler_out.roll;
            }
            else
            { 
                yaw = euler_out2.yaw; 
                pitch = euler_out2.pitch;
                roll = euler_out2.roll;
            }
        }
       /**
         * @brief 将矩阵表示为固定轴XYZ的滚转、俯仰和偏航角
         * @param roll 绕X轴的滚转角
         * @param pitch 绕Y轴的俯仰角
         * @param yaw 绕Z轴的偏航角
         * @param solution_number 解的编号，取值为1或2
         */
        void getRPY(double& roll, double& pitch, double& yaw, unsigned int solution_number = 1) const
        {
            getEulerYPR(yaw, pitch, roll, solution_number);
        }
        /**
         * @brief 将矩阵表示为四元数
         * @param q 四元数
         */
        void getRotation(Quaternion& q) const
	    {
            double trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
            double temp[4];

            if (trace > double(0.0)) 
            {
                double s = tfSqrt(trace + double(1.0));
                temp[3]=(s * double(0.5));
                s = double(0.5) / s;

                temp[0]=((m_el[2].y() - m_el[1].z()) * s);
                temp[1]=((m_el[0].z() - m_el[2].x()) * s);
                temp[2]=((m_el[1].x() - m_el[0].y()) * s);
            } 
            else 
            {
                int i = m_el[0].x() < m_el[1].y() ? 
                    (m_el[1].y() < m_el[2].z() ? 2 : 1) :
                    (m_el[0].x() < m_el[2].z() ? 2 : 0); 
                int j = (i + 1) % 3;  
                int k = (i + 2) % 3;

                double s = tfSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + double(1.0));
                temp[i] = s * double(0.5);
                s = double(0.5) / s;

                temp[3] = (m_el[k][j] - m_el[j][k]) * s;
                temp[j] = (m_el[j][i] + m_el[i][j]) * s;
                temp[k] = (m_el[k][i] + m_el[i][k]) * s;
            }
            q.setValue(temp[0],temp[1],temp[2],temp[3]);
        }
    };
}

#endif
/********************<end>********************/